#ifndef YPL_NonLinearFitting_H
#define YPL_NonLinearFitting_H

#include <functional>
#include <cstdlib>
#include <vector>

#include "Vector.h"
#include "Matrix.h"
#include "LinearEquation.h"

namespace ypl
{


/*!
 * @class NonLinearFitting
 * 非线性拟合问题
 */
class NonLinearFitting
{
public:
    class Model;           // 带参数模型
    class Sample;          // 单样本点
    class Samples;         // 多样本点
    class LMPara;          // L-M 算法系统参数
    class Report;          // 拟合报告
public:
    /*!
     * @brief 构造函数
     * @param model -in- 带参数模型
     * @param samples   -in- 样本
     */
    NonLinearFitting(Model& model, Samples& samples);
    /*!
     * @brief
     */
    void fitting(Report &report, const LMPara &lmPara = LMPara());
protected:
    void LMMethod(Report &report, const LMPara &lmPara = LMPara());
    void calcAlphaBeta(const Vector<Real>& para,
                       Matrix<Real>& alpha, Vector<Real>& beta);
    Real chiSqure(const Vector<Real> &para);
private:
    Model* m_pModel;
    Samples* m_pSamples;

public:
    /*!
     * @brief 带参数数据模型
     */
    class Model : public std::binary_function<Real, const Vector<Real>&, Real>
    {
    public:
        /*! @brief 构造函数 */
        Model(Real (*f)(Real, const Vector<Real>&),
               Integral paraDim,
               const Vector<Real> &lb = Vector<Real>(),
               const Vector<Real> &ub = Vector<Real>(),
               Real dp = 1.0e-10,
               Vector<Real> (*dfdp)(Real x, const Vector<Real>&) = 0);
        /*! @brief 拷贝构造函数 */
        Model(Model& model);
        /*! @brief 带参模型接口 */
        Real operator() (Real x, const Vector<Real>& para) const;
        /*! @brief 赋值 */
        Model& operator =(Model& model);

        /*! @brief 模型对参数的一阶偏导 */
        Vector<Real> dfdp(Real x, const Vector<Real> &para) const;
        /*! @brief 参数维数 */
        Integral paraDim() const;
        /*! @brief 检查参数是否在既定范围内 */
        bool outOfBounds(const Vector<Real> &para) const
        {
            if (! m_useBounds)
                return false;
            if (para.dim() != m_paraDim)
                return true;
            for (int i = 0; i < m_paraDim; ++i) {
                if (para[i] > m_ub[i] || para[i] < m_lb[i])
                    return true;
            }
            return false;
        }
        /*! @brief 在界内生成参数 */
        void genParaInBounds(Vector<Real> &para) const
        {
            if (para.dim() != m_paraDim)
                para = Vector<Real>(m_paraDim);
            // 参数无界
            if (! m_useBounds) {
                for (int i = 0; i < m_paraDim; ++i) {
                    para[i] = 0;
                }
            }
            // 在界内生成参数
            else {
                for (int i = 0; i < m_paraDim; ++i) {
                    para[i] = (m_lb[i] + m_ub[i]) * 0.5;
                }
            }
        }

    protected:
        Vector<Real> numericalDfDp(Real x, const Vector<Real>& para, Real dp) const;
    private:
        Real (*m_f)(Real, const Vector<Real>&); // 带参数模型
        Vector<Real> (*m_dfdp)(Real x, const Vector<Real>& para);  // 模型对参数的导函数
        Integral m_paraDim; // 参数维数
        Vector<Real> m_lb;
        Vector<Real> m_ub;
        bool m_useBounds;
        Real m_dp;
    };


    /*! @brief 单个样本点 */
    class Sample {
    public:
        Sample();
        Sample(Real x, Real y, Real sd = 1.0);
        Real x() const;
        Real y() const;
        Real sd() const;
    private:
        Real m_x;
        Real m_y;
        Real m_sd; // 样本标准差 standard deviation
    };

    /*! @brief 样本 */
    class Samples
    {
    public:
        Samples(Real *xs, Real *ys, Integral size, Real *sd = 0);
        Samples(const std::vector<Real>& xs, const std::vector<Real>& ys,
                 const std::vector<Real>& sd = std::vector<Real>());
        Sample& operator [](Integral index);
        Integral size() const;
        bool useDefaultSD() const;
    private:
        bool m_useDefaultSD;             // 使用默认的标准差 1
        std::vector<Sample> m_vSample;  // 样本
    };
    /*! @brief L-M 算法参数 */
    class LMPara
    {
    public:
        LMPara()
        {
            maxIter = 1000;
            lambda = 1.0e-3;
            step = 10.0;
            chiEPS = 1.0e-3;
        }
    public:
        Integral maxIter; ///< 最大迭代次数
        Real lambda;      ///< 初始 lambda 值
        Real step;        ///< lambda 缩放因子
        Real chiEPS;      ///< 卡方改变量阈值
    };
    /*! @brief 结果报告 */
    class Report
    {
    public:
        Report() {}
        Report(const Vector<Real> &initPara)
            : fittedPara(initPara)
        {}
    public:
        Vector<Real> fittedPara; ///< 拟合得到的参数值
        Real chiSquare;          ///< 卡方值
        Real rmsError;           ///< RMS error, RMS 误差
        Real avgError;           ///< average error, 误差均值
        Real maxError;           ///< maximum error, 误差最大值
        Integral iterCount;      ///< 迭代次数
    };
};


/*******************************************************
 * 类实现：带参模型 class NonLinearFitting::Model
 *******************************************************/

/*! @brief 构造函数 */
inline
NonLinearFitting::Model::Model(
        Real (*f)(Real, const Vector<Real>&),
        Integral paraDim,
        const Vector<Real> &lb,
        const Vector<Real> &ub,
        Real dp,
        Vector<Real> (*dfdp)(Real x, const Vector<Real> &para))
    : m_f(f), m_paraDim(paraDim), m_dp(dp), m_dfdp(dfdp)
{
    m_useBounds = true;
    if (lb.dim() != m_paraDim || ub.dim() != m_paraDim)
        m_useBounds = false;
    else {
        m_lb = lb;
        m_ub = ub;
        for (int i = 0; i < m_paraDim; ++i) {
            if (m_lb[i] > m_ub[i]) {
                m_useBounds = false;
                break;
            }
        }
    }
}

/*! @brief 拷贝构造函数 */
inline
NonLinearFitting::Model::Model(Model& model)
    : m_f(model.m_f),
      m_paraDim(model.m_paraDim),
      m_lb(model.m_lb),
      m_ub(model.m_ub),
      m_dp(model.m_dp),
      m_dfdp(model.m_dfdp),
      m_useBounds(model.m_useBounds)
{}

/*! @brief 带参模型接口 */
inline
Real NonLinearFitting::Model::operator() (Real x, const Vector<Real>& para) const
{
    return m_f(x, para);
}

/*! @brief 赋值 */
inline
NonLinearFitting::Model&
NonLinearFitting::Model::operator =(Model& model)
{
    m_f = model.m_f;
    m_paraDim = model.m_paraDim;
    m_lb = model.m_lb;
    m_ub = model.m_ub;
    m_dp = model.m_dp;
    m_dfdp = model.m_dfdp;
    return *this;
}


/*! @brief 参数维数 */
inline
Integral NonLinearFitting::Model::paraDim() const
{
    return m_paraDim;
}



/*! @brief 求一阶偏导的接口 */
inline
Vector<Real> NonLinearFitting::Model::dfdp(
        Real x,
        const Vector<Real> &para) const
{
    if (m_dfdp != 0)
        return m_dfdp(x, para);
    else {
        return numericalDfDp(x, para, m_dp);

    }
}

/*! @brief 模型对参数的一阶导函数近似 */
inline
Vector<Real>
NonLinearFitting::Model::numericalDfDp(Real x, const Vector<Real>& para,
                                     Real dp) const
{
    const Vector<Real>& p0 = para;
    Vector<Real> dfdp(m_paraDim); // 梯度向量

    Real y0 = m_f(x, p0); // 带参模型在(x,a)处的值
    Vector<Real> p1(p0);
    for (Integral i = 0; i < m_paraDim; ++i) {
        p1[i] += dp;
        Real y1 = m_f(x, p1);
        dfdp[i] = (y1 -y0) / dp; // lim [y(x,a+da) - y(x,a)]/da
        p1[i] -= dp;
    }

    return dfdp;
}



/*********************************************************************
 * 类实现：单样本 class NonLinearFitting::Sample
 *********************************************************************/

inline
NonLinearFitting::Sample::Sample() : m_x(0), m_y(0),m_sd(1) {}

inline
NonLinearFitting::Sample::Sample(Real x, Real y, Real sd)
    : m_x(x), m_y(y), m_sd(sd)
{}

inline
Real NonLinearFitting::Sample::x() const
{
    return m_x;
}

inline
Real NonLinearFitting::Sample::y() const
{
    return m_y;
}

inline
Real NonLinearFitting::Sample::sd() const
{
    return m_sd;
}


/*********************************************************************
 * 类实现：多样本 class NonLinearFitting::Samples
 *********************************************************************/

inline
NonLinearFitting::Samples::Samples(Real *xs, Real *ys, Integral size, Real *sd)
{
    m_vSample = std::vector<NonLinearFitting::Sample>(size);
    if (sd == 0) {
        for (Integral i = 0; i < size; ++i) {
            m_vSample[i] = NonLinearFitting::Sample(xs[i], ys[i]);
        }
        m_useDefaultSD = true;
    }
    else {
        for (Integral i = 0; i < size; ++i) {
            m_vSample[i] = NonLinearFitting::Sample(xs[i], ys[i], sd[i]);
        }
        m_useDefaultSD = false;
    }

}

inline
NonLinearFitting::Samples::Samples(const std::vector<Real>& xs,
                                  const std::vector<Real>& ys,
                                  const std::vector<Real>& sd)
{
    Integral size = (xs.size() < ys.size()) ? xs.size() : ys.size();
    m_vSample.resize(size);
    if (sd.size() < size) {
        for (Integral i = 0; i < size; ++i) {
            m_vSample[i] = NonLinearFitting::Sample(xs[i], ys[i]);
        }
        m_useDefaultSD = true;
    }
    else {
        for (Integral i = 0; i < size; ++i) {
            m_vSample[i] = NonLinearFitting::Sample(xs[i], ys[i], sd[i]);
        }
        m_useDefaultSD = false;
    }
}

inline
NonLinearFitting::Sample&
NonLinearFitting::Samples::operator [](Integral index)
{
    return m_vSample.at(index);
}

inline
Integral NonLinearFitting::Samples::size() const
{
    return m_vSample.size();
}

inline
bool NonLinearFitting::Samples::useDefaultSD() const
{
    return m_useDefaultSD;
}


} // namespace ypl

#endif
